/**
 * @file main.cpp
 * @author ysh
 * @brief ScanMatch
 * @version 0.1
 * @date 2023-05-10
 * 
 * @copyright Copyright (c) 2023
 * 
 */
#include <iostream>
#include <ctime>
#include <Eigen/Core>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/filters/approximate_voxel_grid.h>
#include <pcl/io/pcd_io.h>
#include "../include/models/registration/ndt_registration.hpp"
#include "../include/models/registration/icp_svd_registration.hpp"
#include "../include/models/registration/loam/loam_registration.hpp"
#include "../include/utility/tic_toc.h"
#include "../include/models/loopdetection/LidarIris/LidarIris.h"
#include "../include/models/loopdetection/LidarIris/fftm/fftm.hpp"

int main(int argc, char *argv[]){
    //|step1 读取前后两帧点云
    //.读取上一帧
    CloudData::CLOUD_PTR last_cloud(new CloudData::CLOUD);
    if (pcl::io::loadPCDFile("64line_2.pcd", *last_cloud) == -1)
    {
        PCL_ERROR("Couldn't read file 64line_2.pcd \n");
        return (-1);
    }
    std::cout << "Loaded " << last_cloud->size() << " data points from last time " << std::endl;
    //.读取当前帧
   CloudData::CLOUD_PTR current_cloud(new CloudData::CLOUD);
    if (pcl::io::loadPCDFile("64line_1.pcd", *current_cloud) == -1)
    {
        PCL_ERROR("Couldn't read file 64line_1.pcd \n");
        return (-1);
    }
    std::cout << "Loaded " << current_cloud->size() << " data points from current time" << std::endl;
    //|step2 使用Lidariris进行位姿预测
    LidarIris iris(4, 18, 1.6, 0.75, 50);
    cv::Mat1b li1 = LidarIris::GetIris(*current_cloud);
    cv::Mat1b li2 = LidarIris::GetIris(*last_cloud);
    LidarIris::FeatureDesc fd1 = iris.GetFeature(li1);
    LidarIris::FeatureDesc fd2 = iris.GetFeature(li2);
    int bias;
    auto dis = iris.Compare(fd1, fd2, &bias);
    std::cout<<"bias computed with lidariris is:"<<bias<<std::endl;
    float yaw_diff_rad = bias*M_PI/180.0;
    Eigen::AngleAxisf init_rotation(yaw_diff_rad, Eigen::Vector3f::UnitZ());
    Eigen::Translation3f init_translation(1.79387, 0.720047, 0);
    //Eigen::Translation3f init_translation(0, 0, 0);
    Eigen::Matrix4f init_guess = (init_translation * init_rotation).matrix();
    //|step3 对当前帧进行降采样
    // CloudData::CLOUD_PTR current_filtered_cloud(new CloudData::CLOUD);
    CloudData::CLOUD_PTR current_filtered_cloud(new CloudData::CLOUD);
    pcl::ApproximateVoxelGrid<pcl::PointXYZI> approximate_voxel_filter;
    approximate_voxel_filter.setLeafSize(0.2, 0.2, 0.2);
    approximate_voxel_filter.setInputCloud(current_cloud);
    approximate_voxel_filter.filter(*current_filtered_cloud);
    std::cout << "Filtered cloud contains " << current_filtered_cloud->size()
        << " data points from current time" << std::endl;
    //|step5 开始配准
    CloudData::CLOUD_PTR aligned_cloud(new CloudData::CLOUD);
    Eigen::Matrix4f result_pose;
     std::shared_ptr<RegistrationInterface> registration_ptr_;
    //registration_ptr_= std::make_shared<NDTRegistration>(1.0,0.1,0.01,30);
    registration_ptr_= std::make_shared<ICPSVDRegistration>(0.5,0.01,0.36,50);
    //registration_ptr_= std::make_shared<LOAMRegistration>();
    registration_ptr_->SetInputTarget(last_cloud);
    //registration_ptr_->ScanMatch(current_filtered_cloud,init_guess,aligned_cloud,result_pose);
    TicToc t_r;
    registration_ptr_->ScanMatch(current_cloud,init_guess,aligned_cloud,result_pose);
    printf("配准总时间 %f\n", t_r.toc());
    std::cout<<init_guess<<std::endl;
    std::cout<<result_pose<<std::endl;
    pcl::transformPointCloud(*current_cloud, *aligned_cloud, result_pose);
    //|step6 点云可视化
    std::shared_ptr<pcl::visualization::PCLVisualizer>
          viewer_final(new pcl::visualization::PCLVisualizer("3D Viewer"));
    viewer_final->setBackgroundColor(0, 0, 0);
    //.对上一帧点云着色（红色）并可视化
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZI>
        target_color(last_cloud, 255, 0, 0);
    viewer_final->addPointCloud<pcl::PointXYZI>(last_cloud, target_color, "target cloud");
    viewer_final->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE,
        1, "target cloud");
    //.对NDT转换后的当前帧点云着色（绿色）并可视化
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZI>
        aligned_color(aligned_cloud, 0, 255, 0);
    viewer_final->addPointCloud<pcl::PointXYZI>(aligned_cloud, aligned_color, "aligned cloud");
    viewer_final->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE,
        1, "aligned cloud");
        //.启动可视化
    viewer_final->addCoordinateSystem(1.0);
    viewer_final->initCameraParameters();
    //.等待直到可视化窗口关闭。
    while (!viewer_final->wasStopped())
    {
        viewer_final->spinOnce(100);
    }
    return 0;
}